#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

RF24 radio(7, 8);
const byte address[6] = "00001";

void motor_Init()
{
  pinMode(9,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(A0,OUTPUT);
  pinMode(A1,OUTPUT);
  pinMode(A2,OUTPUT);
  pinMode(A3,OUTPUT);
  pinMode(A4,OUTPUT);
  pinMode(A5,OUTPUT);
  analogWrite(9,0);
  analogWrite(10,0);
  analogWrite(6,0);
  analogWrite(5,0);
  digitalWrite(A0,LOW);
  digitalWrite(A1,LOW);
  digitalWrite(A2,LOW);
  digitalWrite(A3,LOW);
  digitalWrite(A4,LOW);
  digitalWrite(A5,LOW);
  digitalWrite(2,LOW);
  digitalWrite(4,LOW);
}
void setup() {
Serial.begin(9600);
radio.begin();
radio.openReadingPipe(0, address);
radio.setPALevel(RF24_PA_MIN);
radio.startListening();
motor_Init();
}
void go()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,HIGH);
  digitalWrite(A1,LOW);
  digitalWrite(A5,LOW);
  digitalWrite(A4,HIGH);
  //右轮
  digitalWrite(4,LOW);
  digitalWrite(2,HIGH);
  digitalWrite(A2,HIGH);
  digitalWrite(A3,LOW);
}
void left_turn()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,LOW);
  digitalWrite(A1,HIGH);
  digitalWrite(A5,HIGH);
  digitalWrite(A4,LOW);
  //右轮
  digitalWrite(4,LOW);
  digitalWrite(2,HIGH);
  digitalWrite(A2,HIGH);
  digitalWrite(A3,LOW);
}
void back()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,LOW);
  digitalWrite(A1,HIGH);
  digitalWrite(A5,HIGH);
  digitalWrite(A4,LOW);
  //右轮
  digitalWrite(4,HIGH);
  digitalWrite(2,LOW);
  digitalWrite(A2,LOW);
  digitalWrite(A3,HIGH);
}
void right_turn()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,HIGH);
  digitalWrite(A1,LOW);
  digitalWrite(A5,LOW);
  digitalWrite(A4,HIGH);
  //右轮
  digitalWrite(4,HIGH);
  digitalWrite(2,LOW);
  digitalWrite(A2,LOW);
  digitalWrite(A3,HIGH);
}
void left()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,LOW);
  digitalWrite(A1,HIGH);
  digitalWrite(A5,LOW);
  digitalWrite(A4,HIGH);
  //右轮
  digitalWrite(4,LOW);
  digitalWrite(2,HIGH);
  digitalWrite(A2,LOW);
  digitalWrite(A3,HIGH);
}
void right()
{
  analogWrite(9,180);
  analogWrite(10,180);
  analogWrite(6,180);
  analogWrite(5,180);
  //左轮
  digitalWrite(A0,HIGH);
  digitalWrite(A1,LOW);
  digitalWrite(A5,HIGH);
  digitalWrite(A4,LOW);
  //右轮
  digitalWrite(4,HIGH);
  digitalWrite(2,LOW);
  digitalWrite(A2,HIGH);
  digitalWrite(A3,LOW);
}
void stop()
{
  analogWrite(9,0);
  analogWrite(10,0);
  analogWrite(6,0);
  analogWrite(5,0);
  digitalWrite(A0,LOW);
  digitalWrite(A1,LOW);
  digitalWrite(A2,LOW);
  digitalWrite(A3,LOW);
  digitalWrite(A4,LOW);
  digitalWrite(A5,LOW);
  digitalWrite(2,LOW);
  digitalWrite(4,LOW);
}
//if(radio.available()){
//char text[32] ="";
//radio.read(&text, sizeof(text));
//Serial.println(text);
//}
char text[32] ="";
void loop() {
if(radio.available()){
radio.read(&text, sizeof(text));
//小车部分
if(text[0]=='n'){
  while(1)
  {
    if(radio.available()){
      //char text[32] ="";
      radio.read(&text, sizeof(text));
      if(text[0]=='a')
      {
        go();delay(80);
        text[32] ="";
      }
      else  if(text[0]=='b')
      {
        back();delay(80);
        text[32] ="";
      }
      else if(text[0]=='c')
      {
        left();delay(80);
        text[32] ="";
      }
      else if(text[0]=='d')
      {
        right();delay(80);
        text[32] ="";
      }
      else if(text[0]=='e')
      {
        left_turn();delay(150);stop();
        text[32] ="";
      }
      else if(text[0]=='f')
      {
        right_turn();delay(150);stop();
        text[32] ="";
      }
      else
      {
        stop();delay(80);
        text[32] ="";
      }
      if(text[0]=='g')
      {
        break;
      }
    }
  }
  text[32] ="";
  delay(100);
}
//机械臂部分
if(text[0]=='g'){
    while(1){
      if(radio.available()){
      //char text[32] ="";
        radio.read(&text, sizeof(text));
        if(text[0]=='a')
      {
        Serial.println('a');
        text[32] ="";
      }
      else  if(text[0]=='b')
      {
        Serial.println('b');
        text[32] ="";
      }
      else if(text[0]=='c')
      {
        Serial.println('c');
        text[32] ="";
      }
      else if(text[0]=='d')
      {
        Serial.println('d');
        text[32] ="";
      }
      else if(text[0]=='e')
      {
        Serial.println('e');
        text[32] ="";
      }
      else if(text[0]=='f')
      {
        Serial.println('f');
        text[32] ="";
      }
      else if(text[0]=='l')
      {
        Serial.println('l');
        text[32] ="";
      }
      else if(text[0]=='m')
      {
        Serial.println('m');
        text[32] ="";
      }
        if(text[0]=='n')
        {
         break;
        }
      }
      text[32] ="";
      delay(100);
    }
    text[32] ="";
    delay(100);
  }
  }
}